#include "MyMath.h"
#include <cstdlib>
#include <random>
#include <numeric>
#include <iostream>
#include <ctime>
#include "LogCat.h"
/*
** you can also construct a random generator engine from a time-based seed
** unsigned rd = std::chrono::system_clock::now().time_since_epoch().count();//add #include <chrono
**
*/
std::random_device rd;
std::default_random_engine rng{ rd() };

MyMath::MyMath()
{
}


MyMath::~MyMath()
{
}

vector<double> MyMath::GenerateRandomVector(int M)
{
	vector<double> u;
	srand(time(NULL));
	for (int i = 0; i < M;i++) {
		u.push_back(rand()/double(RAND_MAX));
	}
	return u;
}

vector<double> MyMath::multiplier(const vector<double>& uu,int M)
{
	vector<double> multi(M);
	multi[0] = uu[0];
	for (int i = 1; i < M;i++) {
		multi[i] = uu[i] * multi[i - 1];
	}
	return multi;
}
vector<double> MyMath::normalRandomGenerater(float noisy)
{
	std::normal_distribution<> norm_x{ mu_x, noisy*sigma_x };
	std::normal_distribution<> norm_y{ mu_y, noisy*sigma_y };
	std::normal_distribution<> norm_z{ mu_z, noisy*sigma_z };
	std::normal_distribution<> norm_yaw{ mu_yaw, noisy*sigma_yaw };
	std::normal_distribution<> norm_pitch{ mu_pitch, noisy*sigma_pitch };
	std::normal_distribution<> norm_roll{ mu_roll, noisy*sigma_roll };
	vector<double> v{ norm_x(rng), norm_y(rng),norm_z(rng),norm_yaw(rng),norm_pitch(rng),norm_roll(rng) };
	return v;
}

void MyMath::setUniformRandomGenerater(double max_x, double min_x, double max_y, double min_y, double max_z, double min_z)
{
	this->max_x = max_x; 
	this->max_y = max_y;
	this->max_z = max_z;
	this->min_x = min_x;
	this->min_y = min_y;
	this->min_z = min_z;
}

Pose3D& MyMath::particleInitInGlobalMap(Pose3D & particle,double _yaw,double _pitch,double _roll)
{
	std::uniform_real_distribution<double> unif_x{ max_x, min_x };
	std::uniform_real_distribution<double> unif_y{ max_y, min_y };
	std::uniform_real_distribution<double> unif_z{ max_z, min_z };
	particle.x = unif_x(rng);
	particle.y = unif_y(rng);
	particle.z = unif_z(rng);
	particle.yaw = _yaw;
	particle.pitch = _pitch;
	particle.roll = _roll;
	return particle;

}


Pose3D& MyMath::poseDisturbance(Pose3D& pose, float noisy)
{
	vector<double> disturbance(normalRandomGenerater(noisy));
	//LogCat::c("MyMath","poseDisturbance",disturbance);
	/*cout << "MyMath" << "( " << "poseDisturbance" << " ) : ";
	for (auto& p : disturbance) {
		cout << p << " ";
	}
	cout << endl;
	*/
	pose.x += disturbance[0];
	pose.y += disturbance[1];
	pose.z += disturbance[2];
	pose.yaw += disturbance[3];
	pose.pitch += disturbance[3];
	pose.roll += disturbance[3];
	return pose;
}



